#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(ros::roseus "hrp2_petbottle_grasp")

;;(ros::roseus-add-msgs "sensor_msgs")

(load "../pet-grasp-set.l")
(load "package://humanoid_dualarm_modeling/euslisp/observe-handheld-objects.l")
;;(setq *transformer* (instance ros::transform-listener :init))

(defun pet-detect ()
  (my-hook-pose :arms :real t)
  (let ((pet-cds
         (send *transformer* :lookup-transform
               "/HRP2JSKNT/base_link"
               "/plastic_bottle"
               (ros::time 0))))
    (while (not pet-cds)
      (send *transformer* :lookup-transform
            "/HRP2JSKNT/base_link"
            "/plastic_bottle"
            (ros::time 0))
      )
    (let ((tc-lst
           (reverse
            (mapcar #'(lambda (x)
                        (list
                         (car x)
                         (send
                          (send pet-cds :copy-worldcoords)
                          :transform
                          (send (cadr x) :copy-worldcoords))))
                    *pet-grasp-cds-lst*))))
      (dolist (l tc-lst)
        (when (send *hrp2* :rarm :inverse-kinematics
                    (send (send (cadr l) :copy-worldcoords)
                          :translate #f(-50 0 0))
                    :use-torso t)
          (send *hrp2* :rarm :hand :angle-vector (car l))
          (send *ci* :hand-move-joints :rarm (car l) 3000)
          (send *hrp2* :head :look-at (send (send (cadr l) :copy-worldcoords) :worldpos))
          (send *ci* :angle-vector (send *hrp2* :angle-vector))
          (send *ci* :wait-interpolation)
          (my-close-hand3 :rarm :real t)
          (unix:sleep 3)
          (reset-manip-pose)
          (return-from pet-detect t)))
      )
    )
  nil
  )


(defun proc ()
  (setup :real t :impedance t)
  (send *hrp2* :reset-manip-pose)
  (send *ci* :angle-vector (send *hrp2* :angle-vector))
  (send *ci* :wait-interpolation)

  (let ((flag nil))
    (while (not flag)
      (setq flag (pet-detect)))))
;;(proc)